/**
 * @file   a_star_normal.cpp
 * @brief  a* normal
 * @encode UTF-8
 * @log    date        author      notes
 *         2021.09.23  Jimmy       first version
 */
#include "a_star_normal.h"
#include "utils/date_time.h"
#include "debug_log/log.h"

const uint8_t ASTAR_DILATE_SIZE        = 1;
const uint8_t ASTAR_CW_COST_TABEL_SIZE = 4;     /* 靠墙成本 */
const uint8_t ASTAR_EXPEND_CNT         = 8;

const uint8_t ASTAR_STRAIGHT_COST      = 10;
const uint8_t ASTAR_SLASH_COST         = 14;

/* 靠墙惩罚成本 */
const uint8_t closeWallPunishCost[ASTAR_CW_COST_TABEL_SIZE + 1] = {32, 16, 8, 4, 2};
const int8_t  xOffset[8]   = { 1, -1, 0, 0, 1, 1, -1, -1 };
const int8_t  yOffset[8]   = { 0, 0, -1, 1, 1, -1, 1, -1 };
const float   astarCost[8] = { 1.0, 1.0, 1.0, 1.0, 1.4, 1.4, 1.4, 1.4 };


/**
 * @brief  Construct a new AStar::AStar object
 * @author Jimmy 
 * @date   2021.09.23
 */
AStarNormal::AStarNormal()
{
    mOpenList = std::make_unique<Heap>    ();
    mMapList  = std::make_unique<MapList> ();
}

/**
 * @brief  Destroy the AStar::AStar object
 * @author Jimmy 
 * @date   2021.09.23
 */
AStarNormal::~AStarNormal()
{
}

/**
 * @brief  更新地图
 * @param  mapData          
 * @author Jimmy 
 * @date   2021.09.23
 */
void AStarNormal::updateMap(const MapData & mapData)
{
    mWidth   = mapData.param.width;
    mHeight  = mapData.param.height;
    mMapSize = mWidth * mHeight;

    mMapByte.resize(mMapSize, 0);
    mMapByte = mapData.map;

#if 0       /* 像素膨胀 */
    int deltaPos[] = {-1, 1, -mWidth, mWidth};
    uint8_t deltaSize = sizeof(deltaPos) / sizeof(deltaPos[0]);

    int xMin, yMin, xMax, yMax;
    xMin = yMin = 0;
    xMax = mWidth  - 1;
    yMax = mHeight - 1;


    for (int y = 0; y < mHeight; y ++)
    {
        int wid = y * mWidth;

        for (int x = 0; x < mWidth; x ++)
        {
            int i = wid + x;
            /* 值低于255的，全部认为是障碍物 */
            if (mMapByte[i] < 127)
            {
                mMapByte[i] = 0;
            }
            else
            {
                mMapByte[i] = 255;
            }
        }
    }

    for (auto dis = 0; dis < ASTAR_DILATE_SIZE; dis ++)
    {
        for (auto yy = yMin + 1; yy <= yMax - 1; yy ++)
        {
            auto yy_widht = yy * mWidth;

            for (auto xx = xMin + 1; xx <= xMax - 1; xx ++)
            {
                auto i = yy_widht + xx;

                if (mMapByte[i] == dis)
                {
                    for (auto j = 0; j < deltaSize; j ++)
                    {
                        if (mMapByte[i + deltaPos[j]] >= 127)
                        {
                            mMapByte[i + deltaPos[j]] = dis + 1;
                        }
                    }
                }
            }
        }
    }
#endif

    for (int y = 0; y < mHeight; y ++)
    {
        int wid = y * mWidth;

        for (int x = 0; x < mWidth; x ++)
        {
            int i = wid + x;
            /* 值低于255的，全部认为是障碍物 */
            if (mMapByte[i] < 127)
            {
                mMapByte[i] = 0;
            }
            else
            {
                mMapByte[i] = 255;
            }
        }
    }
}

/**
 * @brief  路径规划
 * @param  start            
 * @param  dest             
 * @param  path             
 * @return a_star_normal::astar_status_t 
 * @author Jimmy 
 * @date   2021.09.23
 */
a_star_normal::astar_status_t AStarNormal::pathing(Point & start, Point & dest, std::vector<Point> & path)
{
    log_shell::LOG_INFO("Path finding with AStar Normal\r\n");
    Point zero(0, 0);
    Point maxMap(mWidth, mHeight);

    if (!isPointIn(start, zero, maxMap))
    {
        return a_star_normal::ASS_START_TRAP;
    }

    if (!isPointIn(dest, zero, maxMap))
    {
        return a_star_normal::ASS_START_TRAP;
    }

    if (!canPass(start, 255))
    {
        return a_star_normal::ASS_START_TRAP;
    }

    mMapList->init(mWidth, mHeight);
    mOpenList->clear();

    AStarNPoint startPoint;
    startPoint.pos = start;
    startPoint.g   = 0;
    startPoint.h   = calcH(start, dest);
    startPoint.f   = startPoint.g + startPoint.h;
    mOpenList->push(startPoint);

    mMapList->open(start);
    mMapList->setG(startPoint.g, start);
    mMapList->setOffset(9, start);

    bool isReachTarget = false;

    while(!mOpenList->isEmpty() && !isReachTarget)
    {
        AStarNPoint curPoint = mOpenList->popMin();
        mMapList->close(curPoint.pos);

        for (auto i = 0; i < ASTAR_EXPEND_CNT; i ++)
        {
            Point nextPos(curPoint.pos.x + xOffset[i], curPoint.pos.y + yOffset[i]);

            if (canPass(nextPos, 255) && !mMapList->isClosed(nextPos))
            {
                if (mMapList->isOpen(nextPos))
                {
                    uint16_t temp = calcG(curPoint.pos, nextPos);

                    if (temp < mMapList->getG(nextPos))
                    {
                        mMapList->setOffset(i, nextPos);
                        
                        AStarNPoint nextPoint;
                        nextPoint.g   = temp;
                        nextPoint.h   = calcH(nextPos, dest);
                        nextPoint.f   = nextPoint.g + nextPoint.h;
                        nextPoint.pos = nextPos;
                        mOpenList->erase(nextPoint);
                        mOpenList->push(nextPoint);

                        mMapList->setG(nextPoint.g, nextPos);
                    }
                }
                else
                {
                    mMapList->setOffset(i, nextPos);
                    
                    AStarNPoint nextPoint;
                    nextPoint.g   = calcG(curPoint.pos, nextPos);
                    nextPoint.h   = calcH(nextPos, dest);
                    nextPoint.f   = nextPoint.g + nextPoint.h;
                    nextPoint.pos = nextPos;
                    mOpenList->push(nextPoint);

                    mMapList->open(nextPos);
                    mMapList->setG(nextPoint.g, nextPos);
                                        
                }

                if (nextPos.x == dest.x && nextPos.y == dest.y)
                {
                    isReachTarget = true;
                }
                
            }
        }
    }

    std::vector<Point> tempPoint;

    if (isReachTarget)
    {
        std::vector<Point> points;
        Point nowPoint = dest;

        while(nowPoint.x != start.x || nowPoint.y != start.y)
        {
            points.push_back(nowPoint);
            uint8_t offset = mMapList->getOffset(nowPoint);
            nowPoint.x -= xOffset[offset];
            nowPoint.y -= yOffset[offset];
        }

        points.push_back(nowPoint);

        while (points.size() > 0)
        {
            tempPoint.push_back(points.back());
            points.pop_back();
        }
    }

    if (tempPoint.empty() == false)
    {
        path.clear();
        path = tempPoint;
    }

    log_shell::LOG_INFO("path size:%d\r\n", (int)path.size());

    return a_star_normal::ASS_OK;
}

/**
 * @brief  判断点是否在地图内
 * @param  point            
 * @param  minPoint         
 * @param  maxPoint         
 * @return true 
 * @return false 
 * @author Jimmy 
 * @date   2021.09.23
 */
bool AStarNormal::isPointIn(Point & point, Point & minPoint, Point & maxPoint)
{
    if (point.x < minPoint.x || point.y < minPoint.y || point.x >= maxPoint.x || point.y >= maxPoint.y)
    {
        return false;
    }

    return true;
}

/**
 * @brief  节点可通行
 * @param  point            
 * @return true 
 * @return false 
 * @author Jimmy 
 * @date   2021.09.23
 */
bool AStarNormal::canPass(Point & point, uint8_t ref)
{
    auto i = point.y * mWidth + point.x;

    if (mMapByte[i] < ref)
    {
        return false;
    }

    return true;
}

/**
 * @brief  计算H
 * @param  nPoint          
 * @param  destPoint     
 * @return uint8_t 
 * @author Jimmy 
 * @date   2021.09.23
 */
uint16_t AStarNormal::calcH(Point & nPoint, Point & destPoint)
{
    uint16_t h = 0;

    if (ASTAR_EXPEND_CNT == 4)
    {
        h = abs(destPoint.x - nPoint.x) + abs(destPoint.y - nPoint.y);
    }
    else if (ASTAR_EXPEND_CNT == 8)
    {
        int dx = abs(nPoint.x - destPoint.x);
        int dy = abs(nPoint.y - destPoint.y);

        h = 10 * ((dx + dy) - 2 * std::min(dx, dy)) + 14 * std::min(dx, dy);
    }

    return h;
}

/**
 * @brief  计算G
 * @param  parentPoint          
 * @param  neighborPoint     
 * @return uint8_t 
 * @author Jimmy 
 * @date   2021.09.23
 */
uint16_t AStarNormal::calcG(Point & parentPoint, Point & neighborPoint)
{
    uint16_t g  = ((abs(parentPoint.x - neighborPoint.x) + abs(parentPoint.y - neighborPoint.y)) == 1) ? (ASTAR_STRAIGHT_COST) : (ASTAR_SLASH_COST);
    uint16_t pG = mMapList->getG(parentPoint);

    return (g + pG);
}

/* End of file */
